#!/bin/bash

echo "=== 开始室内点云建图 ==="

# 1. 启动RealSense相机
echo "启动RealSense相机..."
gnome-terminal --title="RealSense" -- bash -c "
ros2 launch realsense2_camera rs_launch.py \
    pointcloud.enable:=true \
    align_depth.enable:=true \
    enable_color:=true \
    enable_depth:=true;
exec bash"

sleep 5

# 2. 启动RTAB-Map建图
echo "启动RTAB-Map建图系统..."
gnome-terminal --title="RTAB-Map" -- bash -c "
ros2 launch rtabmap_launch rtabmap.launch.py \
    rtabmap_args:='--delete_db_on_start' \
    frame_id:=camera_link \
    rgb_topic:=/camera/camera/color/image_raw \
    depth_topic:=/camera/camera/depth/image_rect_raw \
    camera_info_topic:=/camera/camera/color/camera_info \
    approx_sync:=true \
    rtabmap_viz:=true \
    rviz:=true;
exec bash"

sleep 5

# 3. 启动点云保存服务
echo "启动点云保存服务..."
gnome-terminal --title="PointCloud Saver" -- bash -c "
python3 save_pointcloud.py;
exec bash"

echo ""
echo "=== 所有系统已启动 ==="
echo "建图完成后，在新的终端中运行以下命令保存点云："
echo "ros2 service call /save_pointcloud std_srvs/srv/Trigger"
echo ""
echo "或者使用RTAB-Map数据库导出："
echo "ros2 run rtabmap_utils rtabmap_databaseViewer ~/.ros/rtabmap.db"
